package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.KinectStick;


/**
 *
 * @author Lena
 */
public class RobotPayload {
	private int controlMode = CONTROL_DOUBLE_JOYSTICK, trackingMode = TRACKING_MANUAL, shootingMode = SHOOTING_MANUAL;

	public static final int CONTROL_AUTONOMOUS = 0;
	public static final int CONTROL_KINECT = 1;
	public static final int CONTROL_SINGLE_JOYSTICK = 2;
	public static final int CONTROL_DOUBLE_JOYSTICK = 3;
	public static final int CONTROL_GAMEPAD = 4;
	public static final int CONTROL_DISABLED = 5;

	public static final int TRACKING_AUTONOMOUS = 0;
	public static final int TRACKING_AUTOMATIC_CAMERA = 1;
	public static final int TRACKING_MANUAL_PID = 2;
	public static final int TRACKING_MANUAL = 3;
	public static final int TRACKING_DISABLED = 4;

	public static final int SHOOTING_AUTOMATIC = 0;
	public static final int SHOOTING_CONFIDENCE = 1;
	public static final int SHOOTING_MANUAL = 2;
	public static final int SHOOTING_DISABLED = 3;


	private KinectStick kinect;
	private Joystick rightStick, leftStick;
	private BeltSystem beltSystem;
	private Turret turret;

	private int targetSelection = Target.TARGET_TOP;
	private double thetaSetpoint = 0, phiSetpoint = 0, speedSetpoint = 0, speedMotor = 0, confidenceThreshold = 0;
	//TargetPosition targetSelection;

	public RobotPayload(int controlMode, int trackingMode, int shootingMode) {
		this.controlMode = controlMode;
		this.trackingMode = trackingMode;
		this.shootingMode = shootingMode;

		kinect = new KinectStick(1);
		leftStick = new Joystick(Definitions.PAYLOAD_STICK_1);
		rightStick = new Joystick(Definitions.PAYLOAD_STICK_2);

		confidenceThreshold = Definitions.CONFIDENCE_THRESHOLD_DEFAULT;
		thetaSetpoint = 0.0f;
		phiSetpoint = 0.0f;
		speedSetpoint = 0.0f;
		targetSelection = Target.TARGET_NONE;
		beltSystem = BeltSystem.INSTANCE;
		turret = new Turret();
	}

	public void update(){
		double thetaMotorValue, phiMotorValue, speedMotorValue, thetaSetpointChange, phiSetpointChange, speedSetpointChange;
		boolean controlPickup, controlShoot, controlDump;

		thetaMotorValue = phiMotorValue = speedMotorValue = thetaSetpointChange = phiSetpointChange = speedSetpointChange = 0.0;
		controlPickup = controlShoot = controlDump = false;
		targetSelection = Target.TARGET_NONE;

		switch(controlMode) {
		// Autonomous Control without Kinect
		case CONTROL_AUTONOMOUS:
			trackingMode = TRACKING_AUTONOMOUS;

			controlShoot = true;
			controlPickup = true;
			controlDump = false;
			break;

			// Autonomous Control with Kinect
		case CONTROL_KINECT:
			trackingMode = TRACKING_AUTOMATIC_CAMERA;
			shootingMode = SHOOTING_MANUAL;

			targetSelection = Target.TARGET_TOP;

			controlShoot = kinect.getRawButton(1);
			controlPickup = kinect.getRawButton(2);
			controlDump = false;
			break;

		case CONTROL_SINGLE_JOYSTICK:
			thetaMotorValue = -rightStick.getX();
			phiMotorValue = MathExtension.sgn(-rightStick.getY()) * -rightStick.getY() * -rightStick.getY() * Definitions.PHI_MOTOR_MAX;
			speedMotorValue = ((-rightStick.getZ() + 1.0) / 2);

			thetaSetpointChange = rightStick.getX() * Definitions.THETA_STEP;
			phiSetpointChange = MathExtension.sgn(-rightStick.getY()) * -rightStick.getY() * -rightStick.getY() * Definitions.PHI_STEP;
			speedSetpoint = ((-rightStick.getZ() + 1.0) / 2) * Definitions.SPEED_MAX;
			speedSetpointChange = 0.0;

			controlPickup = rightStick.getRawButton(2);
			controlShoot = rightStick.getRawButton(1);
			controlDump = rightStick.getRawButton(3);

			targetSelection = Target.TARGET_NONE;
			break;

		case CONTROL_DOUBLE_JOYSTICK:
			thetaMotorValue = -rightStick.getX();
			phiMotorValue = MathExtension.sgn(-rightStick.getY()) * -rightStick.getY() * -rightStick.getY() * Definitions.PHI_MOTOR_MAX;
			speedMotorValue = Math.max(0.0, -leftStick.getY());

			thetaSetpointChange = rightStick.getX() * Definitions.THETA_STEP;
			phiSetpointChange = MathExtension.sgn(-rightStick.getY()) * -rightStick.getY() * -rightStick.getY() * Definitions.PHI_STEP;
			speedSetpointChange = -leftStick.getY() * Definitions.SPEED_STEP;

			controlPickup = rightStick.getRawButton(2);
			controlShoot = rightStick.getRawButton(1);
			controlDump = rightStick.getRawButton(3);

			if(leftStick.getRawButton(2))
                                targetSelection = Target.TARGET_BOTTOM;
			else if(leftStick.getRawButton(3))
				targetSelection = Target.TARGET_TOP;
			else if(leftStick.getRawButton(4))
				targetSelection = Target.TARGET_LEFT;
			else if(leftStick.getRawButton(5))
				targetSelection = Target.TARGET_RIGHT;
			else
				targetSelection = Target.TARGET_NONE;

			// Fender Shots
			if(leftStick.getRawButton(1)) {
				targetSelection = Target.TARGET_BOTTOM;
				thetaSetpointChange = 0.0;
				//phiSetpoint = Definitions.FENDER_PHI;
				//speedSetpoint = Definitions.FENDER_SPEED;
			}
			break;

		case CONTROL_GAMEPAD:
			if(rightStick.getRawButton(1)) {
				if(rightStick.getRawButton(2)) thetaMotorValue = 0;
				else thetaMotorValue = Definitions.THETA_MOTOR;
			} else if(rightStick.getRawButton(2)) thetaMotorValue = -Definitions.THETA_MOTOR;
			else thetaMotorValue = 0;

			if(rightStick.getRawButton(3)) {
				if(rightStick.getRawButton(4)) phiMotorValue = 0;
				else phiMotorValue = Definitions.PHI_MOTOR;
			} else if(rightStick.getRawButton(4)) phiMotorValue = -Definitions.PHI_MOTOR;
			else phiMotorValue = 0;

			if(rightStick.getRawButton(5)) {
				speedMotor -= 0.05;
			}
			if(rightStick.getRawButton(6)) {
				speedMotor += 0.05;
			}
			speedMotor = Math.min(Math.max(0.0, speedMotor), 1.0);
			speedMotorValue = speedMotor;

			controlPickup = rightStick.getRawButton(12);
			controlShoot = rightStick.getRawButton(11);
			controlDump = rightStick.getRawButton(7);
			break;
		}

		if(thetaSetpointChange > Definitions.THETA_DEADZONE && thetaSetpointChange < -Definitions.THETA_DEADZONE)
			thetaSetpoint += thetaSetpointChange;
		if(phiSetpointChange > Definitions.PHI_DEADZONE && phiSetpointChange < -Definitions.PHI_DEADZONE)
			phiSetpoint += phiSetpointChange;
		if(speedSetpointChange > Definitions.SPEED_DEADZONE && speedSetpointChange < -Definitions.SPEED_DEADZONE)
			speedSetpoint += speedSetpointChange;

		if(thetaSetpoint < Definitions.THETA_MIN) thetaSetpoint = Definitions.THETA_MIN;
		if(thetaSetpoint > Definitions.THETA_MAX) thetaSetpoint = Definitions.THETA_MAX;
		if(phiSetpoint < 0.0) phiSetpoint = 0;
		if(phiSetpoint > Definitions.PHI_MAX) phiSetpoint = Definitions.PHI_MAX;
		if(speedSetpoint < 0.0) speedSetpoint = 0;
		if(speedSetpoint > Definitions.SPEED_MAX) speedSetpoint = Definitions.SPEED_MAX;

		if(controlPickup) beltSystem.pickup();
		if(controlDump) beltSystem.dump();

		switch(shootingMode) {
		case SHOOTING_AUTOMATIC:
			beltSystem.shoot();
			break;

		case SHOOTING_CONFIDENCE:
			if(turret.getConfidence() > confidenceThreshold && controlShoot) beltSystem.shoot();
			break;

		case SHOOTING_MANUAL:
			if(controlShoot) beltSystem.shoot();
			break;
		}

		switch(trackingMode)
		{
		case TRACKING_AUTONOMOUS:
			turret.updatePIDValues(0.0F, Definitions.AUTONOMOUS_PHI_SETPOINT, Definitions.AUTONOMOUS_SPEED_SETPOINT);
			//          turret.updateAutomatic(targetSelection, Definitions.AUTONOMOUS_PHI_SETPOINT, Definitions.AUTONOMOUS_SPEED_SETPOINT);
			break;

		case TRACKING_AUTOMATIC_CAMERA:
			if(targetSelection != Target.TARGET_NONE) {
				turret.updateAutomatic(targetSelection, phiSetpoint, speedSetpoint);
			} else {
				turret.updatePIDValues(thetaSetpointChange, phiSetpoint, speedSetpoint);
			}
			break;

		case TRACKING_MANUAL_PID:
			turret.updatePIDValues(thetaSetpointChange, phiSetpoint, speedSetpoint);
			break;

		case TRACKING_MANUAL:
			turret.updateMotorValues(thetaMotorValue, phiMotorValue, speedMotorValue);
			break;
		}
		beltSystem.update(); 
	}

	public void setControlMode(int mode){
		controlMode = mode;
	}

	public void setTrackingMode(int mode){
		trackingMode = mode;
	}

	public void setShootingMode(int mode){
		shootingMode = mode;
	}
}
